/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.archwood.frc2607.logomotion;

import com.sun.squawk.util.StringTokenizer;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Ron
 */
public class LogomotionDriveTrain implements LogomotionConstants {

    private CANJaguar   frontLeftMotor = null, frontRightMotor = null,
                        rearLeftMotor = null, rearRightMotor = null;

    private Gyro            Gyroscope;
    private AnalogChannel   Temperature;
    private DigitalInput    lineTrackerLeft, lineTrackerMid, lineTrackerRight;

    private double      startTemp, currentTemp, currentGyroAngle;
    private double      frontLeftMotorSpeed, frontRightMotorSpeed,
                        rearLeftMotorSpeed, rearRightMotorSpeed;

    private double      signFrontLeft, signFrontRight,
                        signRearLeft, signRearRight;

    private short       driveNow = 0;

    public final void resetGyro() {
        Gyroscope.reset();
    }
    
    private void initMotors() {
        short retryCount = 0;
        while (frontLeftMotor == null && ++retryCount < 5) {
            try {
                frontLeftMotor = new CANJaguar(frontLeftCANID);
                System.out.println("frontLeft Jag created successfully");
                retryCount = 0;
            } catch (Exception e) {
                System.out.println("frontLeft Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
        retryCount = 0;
        while (rearLeftMotor == null && ++retryCount < 5) {
            try {
                rearLeftMotor = new CANJaguar(rearLeftCANID);
                System.out.println("rearLeft Jag created successfully");
                retryCount = 0;
            } catch (Exception e) {
                System.out.println("rearLeft Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
        retryCount = 0;
        while (frontRightMotor == null && ++retryCount < 5) {
            try {
                frontRightMotor = new CANJaguar(frontRightCANID);
                System.out.println("frontRight Jag created successfully");
                retryCount = 0;
            } catch (Exception e) {
                System.out.println("frontRight Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
        retryCount = 0;
        while (rearRightMotor == null && ++retryCount < 5) {
            try {
                rearRightMotor = new CANJaguar(rearRightCANID);
                System.out.println("rearRight Jag created successfully");
                retryCount = 0;
            } catch (Exception e) {
                System.out.println("rearRight Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
    }

    private void configMotors() {
        boolean keepTrying = true;

        while (keepTrying) {
            try {
               frontLeftMotor.configFaultTime(0.5);
               frontRightMotor.configFaultTime(0.5);
               rearLeftMotor.configFaultTime(0.5);
               rearRightMotor.configFaultTime(0.5);
               
               frontLeftMotor.changeControlMode(CANJaguar.ControlMode.kSpeed);
               frontLeftMotor.setSpeedReference(CANJaguar.SpeedReference.kEncoder);
               frontRightMotor.changeControlMode(CANJaguar.ControlMode.kSpeed);
               frontRightMotor.setSpeedReference(CANJaguar.SpeedReference.kEncoder);
               rearLeftMotor.changeControlMode(CANJaguar.ControlMode.kSpeed);
               rearLeftMotor.setSpeedReference(CANJaguar.SpeedReference.kEncoder);
               rearRightMotor.changeControlMode(CANJaguar.ControlMode.kSpeed);
               rearRightMotor.setSpeedReference(CANJaguar.SpeedReference.kEncoder);

               frontLeftMotor.configEncoderCodesPerRev(frontLeftEncoderLines);
               frontRightMotor.configEncoderCodesPerRev(frontRightEncoderLines);
               rearLeftMotor.configEncoderCodesPerRev(rearLeftEncoderLines);
               rearRightMotor.configEncoderCodesPerRev(rearRightEncoderLines);

               frontLeftMotor.setPID(frontLeftP, frontLeftI, frontLeftD);
               frontRightMotor.setPID(frontRightP, frontRightI, frontRightD);
               rearLeftMotor.setPID(rearLeftP, rearLeftI, rearLeftD);
               rearRightMotor.setPID(rearRightP, rearRightI, rearRightD);

               frontLeftMotor.enableControl();
               frontRightMotor.enableControl();
               rearLeftMotor.enableControl();
               rearRightMotor.enableControl();
               keepTrying = false;
            } catch (Exception e){
                System.out.println("LogomotionDriveTrain.configMotors() exception, retrying...");
                Timer.delay(.1);
            }
        }
    }

    public void mecanumDrive(double forward, double turn, double rotate) {

        updateGyroAndTemp();
        if (++driveNow > 2)  {
            driveNow = 0;
            
            
            frontLeftMotorSpeed = forward + rotate + turn;
            if (frontLeftMotorSpeed < 0.0) signFrontLeft = -1.0;
            else signFrontLeft = 1.0;
            frontLeftMotorSpeed = Math.abs(frontLeftMotorSpeed);

            frontRightMotorSpeed = forward - rotate - turn;
            if (frontRightMotorSpeed < 0.0) signFrontRight = -1.0;
            else signFrontRight = 1.0;
            frontRightMotorSpeed = Math.abs(frontRightMotorSpeed);

            rearLeftMotorSpeed = forward + rotate - turn;
            if (rearLeftMotorSpeed < 0.0) signRearLeft = -1.0;
            else signRearLeft = 1.0;
            rearLeftMotorSpeed = Math.abs(rearLeftMotorSpeed);

            rearRightMotorSpeed = forward - rotate + turn;
            if (rearRightMotorSpeed < 0.0) signRearRight = -1.0;
            else signRearRight = 1.0;
            rearRightMotorSpeed = Math.abs(rearRightMotorSpeed);

            double max = frontLeftMotorSpeed;
            if (frontRightMotorSpeed > max) max = frontRightMotorSpeed;
            if (rearRightMotorSpeed > max) max = rearRightMotorSpeed;
            if (rearLeftMotorSpeed > max) max = rearLeftMotorSpeed;

            if (max > 1.0) {
		frontLeftMotorSpeed /= max;
		frontRightMotorSpeed /= max;
		rearLeftMotorSpeed /= max;
		rearRightMotorSpeed /= max;
            }

            if (frontLeftMotorSpeed > .01)
                frontLeftMotorSpeed = signFrontLeft * (minRPM + (frontLeftMotorSpeed * rangeRPM));
            else frontLeftMotorSpeed = 0;
            if (frontRightMotorSpeed > .01)
                frontRightMotorSpeed = signFrontRight * (minRPM + (frontRightMotorSpeed * rangeRPM));
            else frontRightMotorSpeed = 0;
            if (rearLeftMotorSpeed > .01)
                rearLeftMotorSpeed = signRearLeft * (minRPM + (rearLeftMotorSpeed * rangeRPM));
            else rearLeftMotorSpeed = 0;
            if (rearRightMotorSpeed > .01)
                rearRightMotorSpeed = signRearRight * (minRPM + (rearRightMotorSpeed * rangeRPM));
            else rearRightMotorSpeed = 0;

            try {
                byte syncGroup = 0x20;
                frontLeftMotor.setX(-frontLeftMotorSpeed, syncGroup);
                frontRightMotor.setX(frontRightMotorSpeed, syncGroup);
                rearLeftMotor.setX(-rearLeftMotorSpeed, syncGroup);
                rearRightMotor.setX(rearRightMotorSpeed, syncGroup);
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (Exception e) {
                System.out.println("mecanumDrive exception when setting speeds");
            }
        }
    }
public void mecanumDrivex(double forward, double turn, double rotate) {

        updateGyroAndTemp();

            
            
            frontLeftMotorSpeed = forward + rotate + turn;
            if (frontLeftMotorSpeed < 0.0) signFrontLeft = -1.0;
            else signFrontLeft = 1.0;
            frontLeftMotorSpeed = Math.abs(frontLeftMotorSpeed);

            frontRightMotorSpeed = forward - rotate - turn;
            if (frontRightMotorSpeed < 0.0) signFrontRight = -1.0;
            else signFrontRight = 1.0;
            frontRightMotorSpeed = Math.abs(frontRightMotorSpeed);

            rearLeftMotorSpeed = forward + rotate - turn;
            if (rearLeftMotorSpeed < 0.0) signRearLeft = -1.0;
            else signRearLeft = 1.0;
            rearLeftMotorSpeed = Math.abs(rearLeftMotorSpeed);

            rearRightMotorSpeed = forward - rotate + turn;
            if (rearRightMotorSpeed < 0.0) signRearRight = -1.0;
            else signRearRight = 1.0;
            rearRightMotorSpeed = Math.abs(rearRightMotorSpeed);

            double max = frontLeftMotorSpeed;
            if (frontRightMotorSpeed > max) max = frontRightMotorSpeed;
            if (rearRightMotorSpeed > max) max = rearRightMotorSpeed;
            if (rearLeftMotorSpeed > max) max = rearLeftMotorSpeed;

            if (max > 1.0) {
		frontLeftMotorSpeed /= max;
		frontRightMotorSpeed /= max;
		rearLeftMotorSpeed /= max;
		rearRightMotorSpeed /= max;
            }

            if (frontLeftMotorSpeed > .01)
                frontLeftMotorSpeed = signFrontLeft * (minRPM + (frontLeftMotorSpeed * rangeRPM));
            else frontLeftMotorSpeed = 0;
            if (frontRightMotorSpeed > .01)
                frontRightMotorSpeed = signFrontRight * (minRPM + (frontRightMotorSpeed * rangeRPM));
            else frontRightMotorSpeed = 0;
            if (rearLeftMotorSpeed > .01)
                rearLeftMotorSpeed = signRearLeft * (minRPM + (rearLeftMotorSpeed * rangeRPM));
            else rearLeftMotorSpeed = 0;
            if (rearRightMotorSpeed > .01)
                rearRightMotorSpeed = signRearRight * (minRPM + (rearRightMotorSpeed * rangeRPM));
            else rearRightMotorSpeed = 0;

            try {
                byte syncGroup = 0x20;
                frontLeftMotor.setX(-frontLeftMotorSpeed, syncGroup);
                frontRightMotor.setX(frontRightMotorSpeed, syncGroup);
                rearLeftMotor.setX(-rearLeftMotorSpeed, syncGroup);
                rearRightMotor.setX(rearRightMotorSpeed, syncGroup);
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (Exception e) {
                System.out.println("mecanumDrive exception when setting speeds");
            }
        
    }


    public StringTokenizer motorSPs() {
        String val = Double.toString(frontLeftMotorSpeed * signFrontLeft);
        String str = "SP FL: " + val;
        val = Double.toString(frontRightMotorSpeed * signFrontRight);
        str += " FR: " + val;
        val = Double.toString(rearLeftMotorSpeed * signRearLeft);
        str += "|RL: " + val;
        val = Double.toString(rearRightMotorSpeed * signRearRight);
        str += " RR: " + val + "   ";
        return new StringTokenizer(str, "|");
    }

    public StringTokenizer motorPVs() {
        String str = "PV FL: ";
        try {
            String val = Double.toString(frontLeftMotor.getSpeed());
            str += val;
            val = Double.toString(frontRightMotor.getSpeed());
            str += " FR: " + val;
            val = Double.toString(rearLeftMotor.getSpeed());
            str += "|RL: " + val;
            val = Double.toString(rearRightMotor.getSpeed());
            str += " RR: " + val + "   ";
        } catch (Exception e) {
            str = "PV: cannot read      |PV: cannot read      ";
        }
        return new StringTokenizer(str, "|");
    }

    private void updateGyroAndTemp() {
        currentTemp =(((Temperature.getAverageVoltage()-2.5)*200)+77);
        currentTemp = ((currentTemp-32)*5)/9;
        currentGyroAngle = Gyroscope.getAngle()+((currentTemp-startTemp)*2.5);
    }

    public double getGyroAngle() {
        updateGyroAndTemp();
        return currentGyroAngle;
    }

    public double getCurTemp() {
        updateGyroAndTemp();
        return currentTemp;
    }

    public String getSwitches() {
        String msg =  "L: " + (lineTrackerLeft.get() ? "1" : "0");
        msg += " M: " + (lineTrackerMid.get() ? "1" : "0");
        msg += " R: " + (lineTrackerRight.get() ? "1" : "0");
        return msg;
    }

    public String getTelemetryHeader() {
        String val = "FL SP,FR SP,RL SP,RR SP,FL PV,FR PV,RL PV,RR PV,";
        val += "Gyro,Left,Mid,Right";
        return val;
    }

    public boolean followLine(double speed,double inc)
    {
        boolean left, right, mid;
        left = !lineTrackerLeft.get();
        mid = !lineTrackerMid.get();
        right = !lineTrackerRight.get();

        double DIRECTION = 0;
        boolean atEndOfLine = false;
        if (mid)//Don't Panic, we are on the line!
        {
            if (left && !right)//If we are leaning towards the left...
            {
                //DIRECTION = Math.max(DIRECTION,0);
                DIRECTION+=inc;
                //System.out.println(""+left+","+mid+","+right+": decrease direction, turn left");
                //speed /= 2;
            }
            if (!left && right)//If we are leaning towards the right...
            {
                //DIRECTION = Math.min(DIRECTION,0);
                DIRECTION-=inc;
                //System.out.println(""+left+","+mid+","+right+": increase direction, turn right");
                //speed /= 2;

            }
            if (left && right)//If we are at the end...
            {
                //I'll need to know our direction...
                speed = 0;
                atEndOfLine = true;
                //System.out.println(""+left+","+mid+","+right+": do nothing");
            }
            if (!left && !right)//If we are at the end...
            {
                //I'll need to know our direction...
                //System.out.println(""+left+","+mid+","+right+": go straight");
               //DIRECTION = 0;
            }
        }
        else
        {
            if (left && !right)//Leaning towards the left a lot?
            {
                //DIRECTION = Math.min(DIRECTION,0);
                DIRECTION+=(inc*2);
              //  System.out.println(""+left+","+mid+","+right+": decrease direction, turn left");
                //speed /= 2;

            }
            if (!left && right)//Leaning towards the right a lot?
            {
                //DIRECTION = Math.max(DIRECTION,0);
                DIRECTION-=(inc*2);
            //    System.out.println(""+left+","+mid+","+right+": increase direction, turn right");
               //speed /= 2;
            }
            if (left && right)//We are at the fork!
            {
                //I'll need to decide... for now, lets go left
                //DIRECTION+=inc;
                //System.out.println(""+left+","+mid+","+right+": do nothing");
              //speed /= 2;
            }
            if (!left && !right)//We are completely lost...
            {
                //isOnLine = no;
                //return;
                //System.out.println(""+left+","+mid+","+right+": do nothing");
            }
        }

        mecanumDrive(-speed,0, DIRECTION);
        return atEndOfLine;
    }

    private String getMotorSpeed(CANJaguar m) {
        try { return Double.toString(m.getSpeed()); }
        catch (Exception e) { return "ERR"; }
    }

    public boolean driveToZone(double speed) {
        mecanumDrive(speed,0.0,0.0);
        boolean left, right, mid;
        left = !lineTrackerLeft.get();
        mid = !lineTrackerMid.get();
        right = !lineTrackerRight.get();
        return (left && mid && right);
    }

    public String getTelemetryData() {
        String val = Double.toString(frontLeftMotorSpeed * signFrontLeft) + ",";
        val += Double.toString(frontRightMotorSpeed * signFrontRight) + ",";
        val += Double.toString(rearLeftMotorSpeed * signRearLeft) + ",";
        val += Double.toString(rearRightMotorSpeed * signRearRight) + ",";
        val += getMotorSpeed(frontLeftMotor) + ",";
        val += getMotorSpeed(frontRightMotor) + ",";
        val += getMotorSpeed(rearLeftMotor) + ",";
        val += getMotorSpeed(rearRightMotor) + ",";
        val += Double.toString(currentGyroAngle) + ",";
        val += (lineTrackerLeft.get() ? "Off Line" : "On Line") + ",";
        val += (lineTrackerMid.get() ? "Off Line" : "On Line") + ",";
        val += (lineTrackerRight.get() ? "Off Line" : "On Line");
        return val;
    }

    public LogomotionDriveTrain() {

        driveNow = 0;
        initMotors();
        configMotors();
        lineTrackerLeft = new DigitalInput(leftPhotoSwitchDI);
        lineTrackerMid = new DigitalInput(midPhotoSwitchDI);
        lineTrackerRight = new DigitalInput(rightPhotoSwitchDI);
        Gyroscope = new Gyro(gyroAI);
        Temperature = new AnalogChannel(temperatureAI);
        startTemp = (((Temperature.getVoltage()-2.5)*200)+77);
        startTemp = ((startTemp-32)*5)/9;
        resetGyro();
        updateGyroAndTemp();
    }

}
